#include "state_fixstand.h"
#include "motor_control.h"
#include "global.h"
#include <stdio.h>

#define KP1 6.0f
#define KD1 1.3f

void fixedstand_enter(void)
{
    printf("fixedStand enter\n");

    param.kp = KP1;
    param.kd = KD1;
    param.torque = 0;
    param.velocity = 0;

    for (size_t i = 0; i < 8; i++)
    {
        /* 前腿 */
        if (global.is_foreleg[i] == 1)
        {
            param.position = ANGLE_F * global.is_leftleg[i];
        }
        param.position = ANGLE_B * global.is_leftleg[i];

        motor_set_param(i, &param);
    }
}

#define KP2 8.0f
#define KD2 1.0f

void fixedstand_run(void)
{
    param.kp = KP2;
    param.kd = KD2;
    param.torque = 0;
    param.velocity = 0;

    for (size_t i = 0; i < 8; i++)
    {
        /* 前腿 */
        if (global.is_foreleg[i] == 1)
        {
            param.position = ANGLE_F * global.is_leftleg[i];
        }
        param.position = ANGLE_B * global.is_leftleg[i];

        motor_set_param(i, &param);
    }
}

void fixedstand_exit(void)
{
    printf("fixedStand exit\n");
}